#include "Motor.h"
#include <AFMotor.h>
#include <Arduino.h>


//motor objects are created in the setup function in the main file: MotorController.pde
extern  AF_DCMotor motor1; // create motor #1, 1KHz pwm
extern  AF_DCMotor motor2; // create motor #1, 1KHz pwm
extern  AF_DCMotor motor3; // create motor #1, 1KHz pwm
extern  AF_DCMotor motor4; // create motor #1, 1KHz pwm

void RunMotorBackwards(int time)
{
		motor1.run(BACKWARD);      // turn it on going forward
		delay(time);
		motor1.run(RELEASE);      // turn it on going forward
		delay(time);
}

void Reverse(int speed)
{
	//set the speed of all the motors the same... for now
	motor1.setSpeed(speed);
	motor2.setSpeed(speed);
	motor3.setSpeed(speed);
	motor4.setSpeed(speed);

    motor1.run(BACKWARD);      // turn it on going forward
    motor2.run(BACKWARD);      // turn it on going forward
    motor3.run(BACKWARD);      // turn it on going forward
    motor4.run(BACKWARD);      // turn it on going forward


}

void Forward(int speed)
{
	//set the speed of all the motors the same... for now
	motor1.setSpeed(speed);
	motor2.setSpeed(speed);
	motor3.setSpeed(speed);
	motor4.setSpeed(speed);

    motor1.run(FORWARD);      // turn it on going forward
    motor2.run(FORWARD);      // turn it on going forward
    motor3.run(FORWARD);      // turn it on going forward
    motor4.run(FORWARD);      // turn it on going forward


}

void TurnLeft(int speed)
{

	//set the speed of all the motors the same... for now
	motor1.setSpeed(speed);
	motor2.setSpeed(speed);
	motor3.setSpeed(speed);
    motor4.setSpeed(speed);

    motor1.run(BACKWARD);      // turn it on going forward
    motor2.run(FORWARD);      // turn it on going forward
    motor3.run(BACKWARD);      // turn it on going forward
    motor4.run(FORWARD);      // turn it on going forward

}

void TurnRight(int speed)
{

	//set the speed of all the motors the same... for now
	motor1.setSpeed(speed);
	motor2.setSpeed(speed);
	motor3.setSpeed(speed);
    motor4.setSpeed(speed);

    motor1.run(FORWARD);      // turn it on going forward
    motor2.run(BACKWARD);      // turn it on going forward
    motor3.run(FORWARD);      // turn it on going forward
    motor4.run(BACKWARD);      // turn it on going forward

}

void Stop()
{
		motor1.run(RELEASE);      // turn it on going forward
		motor2.run(RELEASE);      // turn it on going forward
		motor3.run(RELEASE);      // turn it on going forward
		motor4.run(RELEASE);      // turn it on going forward
}